#include "Track.h"

volatile  uint8_t Track_State = 0;
uint8_t Over_Flag = 0;

volatile Track_HonWai_Type Border_Data = {
    0,
    0,
    0,
};

// 返回五路传感器状态（仅使用IN1-IN5）
uint8_t Track_Read_State(void) 
{
    uint8_t status = 0;
    status |= (IN_X1 << 4);  // IN1作为最高位（第4位）
    status |= (IN_X2 << 3);  // IN2（第3位）
    status |= (IN_X3 << 2);  // IN3（第2位）
    status |= (IN_X4 << 1);  // IN4（第1位）
    status |= (IN_X5 << 0);  // IN5作为最低位（第0位）
    return status;
}


//对数据进行中值滤波处理 并返回处理后的数据
uint8_t Track_Median_Filter_Handle(void)
{
    uint8_t Data[9];   
    for(uint8_t i = 0; i < 9; i++) {
        Data[i] = Track_Read_State();
    }
    Data_Conver_Bin(Data[4]);  // 输出中值
    return Data[4];
}


// 调整输出为5位二进制格式
void Data_Conver_Bin(uint8_t value)
{
    char Data_Bin[6]; // 5位 + 结束符
    for (int i = 4; i >= 0; i--) {
        Data_Bin[4 - i] = (value & (1 << i)) ? '1' : '0';
    }
    Data_Bin[5] = '\0';
    Sprintf("%s\r\n", Data_Bin);  
}


void Track_Contorl(uint8_t State)
{
    // static uint8_t Buzzer_State = 0;
    // static uint8_t Last_Buzzer_State = 0;
    // static uint8_t CNT = 0;
    
    // if(Contrl_Flag == Track_Contrl)
    // {
    //     if(State == 0x00)       //未检测到黑线
    //     {      
    //         vMPU_Contrl();
    //         Buzzer_State = 0;
    //     }else if (State != 0x00)    //检测到黑线
    //     {
    //         Buzzer_State = 1;
    //         vTrack_Base(State);
    //     }

    //     if (Buzzer_State != Last_Buzzer_State) 
    //     {
    //         Buzzer_Flag = 1;
    //         Last_Buzzer_State = Buzzer_State;
    //         CNT ++;
    //         if(CNT == 6)
    //         {
    //             CNT = 0;
    //             Contrl_Flag = 0;
    //             Motor_Control(0, 0);
    //         }
    //     }
    // }
}

// 更新为五路传感器状态表  A左  B右
void vTrack_Base(uint8_t State)
{
    switch (State) 
    {
        // 左侧检测（IN1/IN2）
        case 0x10: Motor_Control(Base_Value , Base_Value + 300); break;  // 10000
        case 0x18: Motor_Control(Base_Value , Base_Value + 200); break;  // 11000
        case 0x08: Motor_Control(Base_Value , Base_Value + 180); break; // 01000
        case 0x0C: Motor_Control(Base_Value , Base_Value + 100); break; // 01100
        
        // 居中（IN3）
        case 0x04: Motor_Control(Base_Value, Base_Value); break;       // 00100
        
        // 右侧检测（IN4/IN5）
        case 0x06: Motor_Control(Base_Value + 100, Base_Value); break; // 00110
        case 0x02: Motor_Control(Base_Value + 180, Base_Value); break; // 00010
        case 0x03: Motor_Control(Base_Value + 200, Base_Value); break; // 00011
        case 0x01: Motor_Control(Base_Value + 300, Base_Value); break; // 00001
        
        // 特殊状态
        case 0x1F: if(!Over_Flag) Motor_Control(0,0); break;  // 11111 (全黑)
        case 0x00: /* 无操作 */ break;                         // 00000 (全白)
        
        // 过渡状态处理
        case 0x14: // 10100
        case 0x0E: // 01110
        default: 
            Motor_Control(Base_Value, Base_Value);  // 默认直行
    } 
}


// PID误差计算更新
uint8_t Track_Calc_Err_Pid(uint8_t state)
{ 
    int8_t cur_error = 0;

    switch (state) 
    {
        // 左侧误差（正值）
        case 0x10: cur_error = 2; break;   // 10000
        case 0x18: cur_error = 4; break;   // 11000
        case 0x08: cur_error = 6; break;   // 01000
        case 0x0C: cur_error = 8; break;   // 01100
        
        // 居中（无误差）
        case 0x04: cur_error = 0; break;   // 00100
        
        // 右侧误差（负值）
        case 0x06: cur_error = -4; break;  // 00110
        case 0x02: cur_error = -6; break;  // 00010
        case 0x03: cur_error = -8; break;  // 00011
        case 0x01: cur_error = -10; break; // 00001
    }
    
    return cur_error;
}